/*
 * @Description:
 * @Version:
 * @Author: Troubadour
 * @Date: 2022-10-11 19:07:43
 * @LastEditors: Troubadour
 * @LastEditTime: 2022-10-11 19:49:05
 */

#include "kalman.h"


/**
 * @function: kalman_Filter
 * @brief 卡尔曼一阶滤波
 * @param {kalmanTypeDef} *kalman
 * @param {double} value
 * @return {*}
 */
void kalman_Filter(kalmanTypeDef *kalman, double value)
{
    // 预测协方差方程
    kalman->Now_P = kalman->Last_P + kalman->Q;
    // 卡尔曼增益方程
    kalman->K = kalman->Now_P / (kalman->Now_P + kalman->R);
    // 更新最优值方程
    kalman->Out = kalman->Out + (kalman->K * (value - kalman->Out));
    // 更新协方差方程
    kalman->Last_P = (1 - kalman->K) * kalman->Now_P;
}


/**
 * @function: Kalman_init
 * @brief 将卡尔曼结构体和一个当前的稳定值初始化
 * @param {kalmanTypeDef} *kalman: 卡尔曼结构体
 * @param {double} value: 初始值
 * @return {*}
 */
void Kalman_init(kalmanTypeDef *kalman, double value)
{
    uint8_t i = 0;
    double diff = 0;

    for(i = 0; i < KALMAN_INIT_CNT; i++)
    {
        kalman_Filter(kalman, value);
        diff = kalman->Out / value;
        diff = (diff > 1) ? (diff - 1) : (1 - diff);
        if (diff < KALMAN_INIT_SOC)
        {
            break;
        }
    }
}







